#include <ros/ros.h>
#include "can_ros1_driver/CarState.h"
#include <mysql/mysql.h>
#include <cmath>

// 连接数据库，并订阅小车状态数据，保存到数据库中
class CarDataSubscriber
{
private:
    ros::NodeHandle nh;
    ros::Subscriber sub;
    MYSQL *conn;

public:
    CarDataSubscriber()
    {
        // 初始化MySQL连接
        conn = mysql_init(nullptr);
        if (!conn)
        {
            ROS_ERROR("Error initializing MySQL connection");
            return;
        }

        // 连接MySQL数据库
        // "root":数据库管理员 "123":root密码 "Car":数据库的名字, Car数据库事先在终端创建：
        // | car_data | CREATE TABLE `car_data` (
        //     `timestamp` timestamp NULL DEFAULT NULL,
        //     `driving_mode` int DEFAULT NULL,
        //     `gear` int DEFAULT NULL,
        //     `brake` int DEFAULT NULL,
        //     `steering_machine_state` int DEFAULT NULL,
        //     `voltage_state` int DEFAULT NULL,
        //     `motor_state` int DEFAULT NULL,
        //     `steering` float DEFAULT NULL,
        //     `speed` float DEFAULT NULL,
        //     `throttle_opening` int DEFAULT NULL,
        //     `acceleration` float DEFAULT NULL,
        //     `battery_power` int DEFAULT NULL,
        //     `can_statu` tinyint(1) DEFAULT NULL,
        //     `mwlidar_statu` tinyint(1) DEFAULT NULL,
        //     `ultrasonic_statu` tinyint(1) DEFAULT NULL,
        //     `lidar_statu` tinyint(1) DEFAULT NULL,
        //     `camera_statu` tinyint(1) DEFAULT NULL
        //     ) ENGINE=InnoDB DEFAULT CHARSET=utf8mb4 COLLATE=utf8mb4_0900_ai_ci |
        if (mysql_real_connect(conn, "localhost", "root", "123", "car", 0, nullptr, CLIENT_FOUND_ROWS))
        {
            ROS_INFO("\033[32mCar database connect success ...\033[0m");

            // // 计算一个月前的时间戳
            // double oneMonthAgo = ros::Time::now().toSec() - 2592000; // 2592000秒 = 30天
            // // 删除一个月前的数据
            // std::string deleteQuery = "DELETE FROM car_data WHERE timestamp < FROM_UNIXTIME(" + std::to_string(oneMonthAgo) + ")";

            
           



            sub = nh.subscribe("/feedback_carState", 1, &CarDataSubscriber::carDataCallback, this);
        }
        else
        {
            ROS_ERROR("Error connecting to Car database: %s", mysql_error(conn));
            return;
        }
    }

    void carDataCallback(const can_ros1_driver::CarState::ConstPtr &msg)
    {
        std::stringstream ss;
        ss << "INSERT INTO car_data (driving_mode, gear, brake, steering_machine_state, voltage_state, motor_state, steering, speed, throttle_opening, acceleration, battery_power, can_statu, mwlidar_statu, ultrasonic_statu, lidar_statu, camera_statu) VALUES ("
           << msg->driving_mode << ", "
           << msg->gear << ", "
           << msg->brake << ", "
           << msg->steering_machine_state << ", "
           << msg->voltage_state << ", "
           << msg->motor_state << ", "
           << msg->steering << ", "
           << msg->speed << ", "
           << msg->throttle_opening << ", "
           << msg->acceleration << ", "
           << msg->battery_power << ", "
           << static_cast<int>(msg->can_statu) << ", "
           << static_cast<int>(msg->mwlidar_statu) << ", "
           << static_cast<int>(msg->ultrasonic_statu) << ", "
           << static_cast<int>(msg->lidar_statu) << ", "
           << static_cast<int>(msg->camera_statu) << ")";

        if (mysql_query(conn, ss.str().c_str()) != 0)
        {
            ROS_ERROR("Error inserting data into MySQL table: %s", mysql_error(conn));
        }
    }

    ~CarDataSubscriber()
    {
        // 关闭MySQL连接
        if (conn)
        {
            mysql_close(conn);
        }
    }
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "Car_data_saver");

    CarDataSubscriber subscriber;

    ros::spin();

    return 0;
}
